#include "main.h"
#include "bsp_can.h"


extern CAN_HandleTypeDef hcan1;
//extern CAN_HandleTypeDef hcan2;


void can_filter_init(void) {
	
	CAN_FilterTypeDef can_filter_st;
  can_filter_st.FilterActivation = ENABLE;
  can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
  can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
  can_filter_st.FilterIdHigh = 0x0000;
  can_filter_st.FilterIdLow = 0x0000;
  can_filter_st.FilterMaskIdHigh = 0x0000;
  can_filter_st.FilterMaskIdLow = 0x0000;
  can_filter_st.FilterBank = 0;
	can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
	HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
	HAL_CAN_Start(&hcan1);
  HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);


  can_filter_st.SlaveStartFilterBank = 14;
  can_filter_st.FilterBank = 14;
  //HAL_CAN_ConfigFilter(&hcan2, &can_filter_st);
  //HAL_CAN_Start(&hcan2);
  //HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO0_MSG_PENDING);

}

motor_measure_t motor_data[6];

static CAN_TxHeaderTypeDef  gimbal_tx_message;
static uint8_t              gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];


void CAN_cmd_chassis(int16_t *speed) {
	uint32_t send_mail_box;
	chassis_tx_message.StdId = CAN_3508_ALL_ID;
	chassis_tx_message.IDE = CAN_ID_STD;
	chassis_tx_message.RTR = CAN_RTR_DATA;

	chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = *(speed) >> 8;
	chassis_can_send_data[1] = *(speed);
	chassis_can_send_data[2] = *(speed + 1) >> 8;
	chassis_can_send_data[3] = *(speed + 1);
	chassis_can_send_data[4] = *(speed + 2) >> 8;
	chassis_can_send_data[5] = *(speed + 2);
	chassis_can_send_data[6] = *(speed + 3) >> 8;
	chassis_can_send_data[7] = *(speed + 3);
	
	HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

void CAN_cmd_gimbal(int16_t *speed) {
	uint32_t send_mail_box;
	chassis_tx_message.StdId = CAN_6020_ALL_ID;
	chassis_tx_message.IDE = CAN_ID_STD;
	chassis_tx_message.RTR = CAN_RTR_DATA;

	chassis_tx_message.DLC = 0x08;
	chassis_can_send_data[0] = *(speed) >> 8;
	chassis_can_send_data[1] = *(speed);
	chassis_can_send_data[2] = *(speed + 1) >> 8;
	chassis_can_send_data[3] = *(speed + 1);
	chassis_can_send_data[4] = *(speed + 2) >> 8;
	chassis_can_send_data[5] = *(speed + 2);
	chassis_can_send_data[6] = *(speed + 3) >> 8;
	chassis_can_send_data[7] = *(speed + 3);
	
	HAL_CAN_AddTxMessage(&hcan1, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

#define get_motor_data(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }

		
#define IScare \
		rx_header.StdId == CAN_3508_M1_ID ||  \
		rx_header.StdId == CAN_3508_M2_ID ||  \
		rx_header.StdId == CAN_3508_M3_ID ||  \
		rx_header.StdId == CAN_3508_M4_ID
		
		
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
	CAN_RxHeaderTypeDef rx_header;
  uint8_t rx_data[8];

  HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
  
	if(rx_header.StdId == CAN_3508_M1_ID || rx_header.StdId == CAN_3508_M2_ID || rx_header.StdId == CAN_3508_M3_ID || rx_header.StdId == CAN_3508_M4_ID) {
		uint8_t i = rx_header.StdId - CAN_3508_M1_ID;
    get_motor_data(&motor_data[i], rx_data);
	}
	
	if(rx_header.StdId == CAN_6020_M1_ID || rx_header.StdId == CAN_6020_M2_ID) {
		uint8_t i = rx_header.StdId - CAN_6020_M1_ID + 4;
    get_motor_data(&motor_data[i], rx_data);
	}
}
